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Abstract 

Current virtual environment and teleoperator applica - 
tions are hampered by the need for an accurate, quick- 
responding head-tracking system with a large working 
volume. Gyroscopic orientation sensors can overcome 
problems with jitter , latency, interference, line-of-sight 
obscurations, and limited range, but suffer from slow 
drift. Gravimetric inclinometers can detect attitude 
without drifting, but are slow and sensitive to transverse 
accelerations. This paper describes the design of a 
Kalman filter to integrate the data from these two types of 
sensors in order to achieve the excellent dynamic 
response of an inertial system without drift, and without 
the acceleration sensitivity of inclinometers . 


1. Introduction 

One of the key technological challenges in virtual 
environment, teleoperator, and augmented reality systems 
is head-tracking. Noise and latency in the data output by 
most current magnetic, acoustic, and optical head- 
tracking systems cause the objects in the virtual world to 
appear jittery and to swim about their correct stationary 
positions during head movements. Range limitations 
prohibit the use of VR for applications such as out-door 
operations training or building walkthroughs. Interference 
and distortions, particularly in magnetic systems, can 
cause user disorientation [1-3]. 

In order to overcome problems of limited range, port- 
ability, and line-of-sight restrictions, some kind of self- 
contained sourceless tracking system would be highly 
desirable. A purely inertial tracker would have the 
additional advantages of nearly instantaneous measure- 
ment, availability of motion derivatives for prediction, 
superb resolution/negligible jitter, and immunity to all 
forms of interference. 

The operating principles for measuring orientation and 
position of a moving body using only gyroscopes and 


accelerometers have been well established in the field of 
Inertial Navigation Systems(INS) [4-9]. The variant 
called strapdown INS measures the orientation of a body 
by integrating the angular rates from three orthogonal 
rate gyros affixed to the body, starting from a known 
initial orientation. This orientation subsystem is referred 
to as an Attitude and Heading Reference System (AHRS). 
To get position, 3 linear accelerometers, also affixed to 
orthogonal axes of the moving body, measure the total 
acceleration vector of the body relative to inertial space. 
This acceleration vector can be converted from body 
coordinates to earth coordinates using the known 
instantaneous orientation of the body determined by the 
AHRS. Position is then obtained by subtracting off the 
effect of gravity from the measured acceleration and then 
performing double integration starting from a known 
initial position. 

Drift in the determination of orientation by the AHRS 
results from gyro biases, which lead to a linear drift rate 
after single integration. If the startup bias can be meas- 
ured and nulled, the worst case drift rate is determined by 
the bias stability, which ranges from about l°/$econd for 
inexpensive silicon micromachined gyros to 0.001°/hour 
for sophisticated inertial navigation gyros. The best gyros 
of a practical size for head-tracking have a bias stability 
on the order of Earth’s rotation rate of 15°/hour. Much 
less expensive and smaller are miniature vibrating 
element gyros with bias stabilities of several de- 
grees/minute and worse. Drift in the measurement of 
linear displacement is a far more difficult problem due to 
the double integration of acceleration, and is not ad- 
dressed in this paper. 

An inertial head-tracker has been developed by the 
author at MIT, concentrating first on the more tractable 
problem of 3-DOF orientation tracking [10]. The first 
prototype consisted of three orthogonal angular rate 
sensors together with a two-axis fluid inclinometer for 
drift compensation. The outputs of the angular rate 
sensors were integrated to obtain orientation, and the 
orientation was occasionally reset by the fluid inclinome- 
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Figure 1: MIT inertial tracker 2nd prototype 

ter to correct for the slow drift of the gyros. Due to the 
relatively high performance of the rate transducers used in 
that prototype, even this simple sensor fusion algorithm 
was able to achieve orientation tracking performance of 
<1 ms latency, 0.008° r.m.s. noise, and 0.5° absolute 
static and dynamic accuracy in pitch and roll [11]. At 1 
lb., the prototype was still a little large for practical head- 
tracking applications. 

A second prototype of the MIT Inertial Tracker, shown 
in Figure 1, has now been built which incorporates tiny 
low-cost solid-state rate gyros, a two-axis fluid inclinome- 
ter, and a two-axis fluxgate compass. It weighs only 3.5 
ounces, can be comfortably worn on a head-mounted 
display, and uses low-cost sensors so that it can be 
developed into a competitive commercial head-tracking 
product. However, the miniature low-cost rate gyros have 
so much higher hysteresis, nonlinearity and bias instabil- 
ity that the simple ad hoc drift correction algorithm used 
in the previous prototype does not lead to sufficiently 
accurate results. This paper concerns the design of a more 
sophisticated sensor data fusion scheme, based on Kalman 
filtering, which makes the best use of all the data 
available from both types of sensors and thereby achieves 
a lower mean squared orientation estimation error than 
the ad hoc method. To be useful, the filter must be able to 
run in real time on an inexpensive 486-class microproces- 
sor, so considerable effort is invested in formulating a 
minimum-order Kalman filter and implementing it 
efficiently. 

The main contributions of this paper are 1) an analysis 
of the literature about related Kalman filter applications, 
2) an exposition of the modeling decisions that were made 
to formulate the filter, which will help others to frame the 
questions necessary to apply Kalman filtering to similar 
problems, 3) an example of the use of Friedland's 
separate-bias Kalman filter formulation, which has not 
been previously applied in synthetic environment tracking 


work, and 4) a very effective adaptive algorithm for 
adjusting the Kalman filter parameters to the instantane- 
ous motion characteristics. This paper focuses more on 
filter design and implementation than validation, and no 
effort is made to formulate an optimal filter and compare 
the performance of the reduced-order filter to the optimal 
filter in simulation. 

2. Kalman filtering 

Consider a dynamic system which can be modeled by a 
n-by-1 state vector x obeying a discrete-time (DT) 
evolution equation 

x 4M = Ax t + Bu t + w t (1) 

where A is an n-by-n state transition matrix, B is an n-by- 
p matrix and u is a p-by-1 vector of known system inputs, 
and w is an n-by-1 process noise vector with covariance 
matrix Qk- (Note that lower-case bold letters, Greek or 
Roman, denote vectors, and upper-case bold letters denote 
matrices.) Suppose there are indirect measurements of the 
state vector available at each time k, and that they can be 
expressed as an m-by-1 measurement vector 

y* =Cx, +v t (2) 

where C is an m-by-n system observation matrix, and v is 
an m-by-1 measurement noise vector with covariance R*. 
A Kalman filter is a recursive algorithm for computing an 
estimate x of state which is optimal in the sense of least 
square error under certain circumstances. One form of the 
DT Kalman filter, used in Section 4.5, is 

+Bu, + K t+1 (y 1+ , -CAx t ) (3) 

where the Kalman gain matrix K is computed from the 
estimation error covariance matrix, P, according to 

K, = P,C r [CP,C r + R, ]'' (4) 

and P is updated according to the Ricatti equation: 

P t+1 =A[l-K,C]P t A r +Q, +1 . (5) 

The Kalman filter is very useful for combining data 
from several different indirect and noisy measurements to 
try to estimate variables which are not directly measur- 
able. Thus, while the gyroscopes measure orientation by 
integrating angular rates, and the inclinometer and 
compass provide a different noisy and sloshy but drift-free 
measurement of orientation, the Kalman filter weights 
the two sources of information appropriately to make the 
best use of all the data from each. If the model in (1) and 
(2) is a simplification of the actual physical system, the 
resulting reduced-order Kalman filter (ROKF) will not be 
optimal, but will often perform almost as well as the full- 
order Kalman filter. This property is exploited in this 
paper without any attempt to evaluate the performance of 
the ROKF. If the system dynamics are nonlinear, it is 
possible to linearize about a nominal or actual trajectory 
and run a Kalman filter on the linearized system. This is 
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the basis of the extended Kalman filter (EKF) and the 
complimentary Kalman filter developed in Section 4.2. A 
discussion of Kalman filtering can be found in [12]. 

3. Literature analysis 

In applying Kalman filtering to the inertial orientation 
tracking problem there is considerable freedom in system 
modeling - what physical variables to assign to the state 
vector x, what measurements are in the measurement 
vector y, and what matrices A, B, C, Q, and R most 
accurately describe the system given those choices. A 
literature search was conducted to see how other authors 
have used Kalman filters to estimate orientation from the 
outputs of 3 strapdown gyros. The 7 most relevant 
references found are reviewed in this section. Two come 
from vehicle navigation, two from robotics, and three 
from virtual environments. 

An early maritime navigation work by Bona and Smay 

[13] , summarized in [12], is of interest because it shows 
how to reset gyro biases based on indirect measurements 
(position errors that result from them) and provides a 
now-common Markov model of gyro bias evolution. The 
dynamic system model details how the position errors 
evolve in response to the gyro biases, and how the gyro 
bias Markov components evolve in response to the 
process noise. 

The most relevant reference found in the aeronautics 
literature was Koifman and Merhav's description of an 
autonomously aided strapdown attitude reference system 

[14] . Here, an autopilot is created with three low-cost rate 
gyros with time-varying biases on the order of 0.1°/s. The 
measurements fed into the Kalman filter are from the 
three gyros, a magnetic compass, altimeter, and airspeed 
sensor. The state vector contains 16 elements: 3 linear 
velocities, 3 angular velocities, 3 orientation Euler angles, 
altitude, 3 wind gust velocity components and 3 gyro 
biases. The state transition matrix is obtained by lineariz- 
ing the system differential equations which encompass 
the aircraft equations of motion as well as the kinematic 
Euler equations (6). In contrast to Bona and Smay, the 
gyro biases are considered piecewise constant, and the 
corresponding diagonal covariance elements are simply 
reset whenever a change detection algorithm suspects that 
the gyro biases may have changed. It is also instructive to 
note that the full order 16-dimensional system could not 
be run in real time, so they reduced the state to 11 
elements and were then able to achieve about 20 updates 
per second with minimal loss in accuracy. The measure- 
ment vector consists of the three gyros and the airspeed 
sensor. 

Barshan and Durrant-Whyte [15] investigated the use 
of a solid-state gyroscope for mobile robotics applications. 


They paid particular attention to the gyroscope error 
model, and came up with an exponential curve to fit the 
changes in bias as the gyroscope warms up. They then 
implemented a Kalman filter for estimating a single 
rotation angle with a state vector 

<£> <i> <i> d> e# and a state transition matrix 

that propagates the truth states and error states 

completely independently. The only system 

observation is the single rate gyro measurement, so the 
system is not observable, and the angular position error 
covariance grows unbounded. However, it is demonstrated 
that the gyro drift error grows at a rate 5 times slower 
when using the exponential gyro error model. 

A paper on mobile robot attitude estimation by Vaga- 
nay et al [16] provides the only example in the literature 
in which gyroscope drift is compensated using two 
accelerometers, and is therefore particularly germane to 
this drift-free head-tracking application. The Kalman 
filter model is very unusual and results in a state vector of 
surprisingly low dimension. The integration of angular 
rates is done outside of the Kalman filter, and is treated as 
part of a measurement system that provides gyroscopically 
determined measurements of pitch and roll, 0 g and y g , 
which are complimented by gravimetric measurements of 
0 and \|/ from the accelerometers. The state contains 0 and 
y and the pitch and roll drift rates, and the transition 
matrix used in the Kalman filter is just the identity. This 
is the leanest Kalman filter conceivable, as even the 
kinematics of Euler angle integration are not modeled, but 
the performance reported is nearly comparable to the 
other methods. No details are given about the determina- 
tion of Q and R. 

Azuma and Bishop developed a Kalman filter to use 
inertial sensors together with an optical head-tracker to 
predict head motion in HMD applications [17]. The 
approach is different from the preceding papers, and also 
from the application developed in this paper, because the 
gyroscope rate signals are not integrated to obtain 
orientation. Instead, the orientation is obtained from the 
optical head-tracker, and the angular rates are fused with 
this in the Kalman filter to yield improved predictions. 
The state vector contains a quaternion specifying 
orientation, the angular rates in body axes, and the 
angular accelerations in body axes. The measurement 
consists of the quaternion measured by the optical tracker, 
and the angular rates measured by the gyros. The Q and 
R matrices are determined off-line using Powell's method 
on prerecorded datasets to find the parameters that give 
the best performance. Prediction was accomplished by 
extrapolating forward in time, using the angular velocity 
and acceleration estimates in the state vector. 
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Emura and Tachi likewise used gyros to augment the 
dynamic performance of an existing head-tracker, but in 
this case the tracker was magnetic instead of optical [18, 
19]. The state vector contains orientation (Euler angles in 
the first paper were replaced with a quaternion in the 
second) and angular velocities. The measurement vector 
measures all elements of the state, using a Polhemus 
magnetic tracker to measure orientation and gyros to 
measure the angular rates. A novel aspect of the Kalman 
filter structure is the use of two different types of meas- 
urement update step: a 3-dimensional measurement used 
most of the time, when only gyro data is available, and a 
6-dimensional measurement used when the Polhemus 
data is available as well. Q and R were found empirically, 
using a high-precision mechanical tracker as a reference 
to measure remnant errors. 

4. System modeling and filter design 


4.1 State and measurement vectors 


The first step in modeling is to decide what to put in 
the state and measurement vectors. Since the basic 
purpose of the Kalman filter is to estimate orientation, it 
is a given that it will be included in the state vector. 
Indeed, all the authors except [13] include it, although 
they are split between quaternion and Euler angle 
representations. In the interest of smaller state dimension 
(i.e. faster computation), this implementation uses Euler 
angles. The aeronautics convention is used, where 0, 
and \| called yaw, pitch and roll respectively, represent 
positive rotations about the z, y, and x body axes in turn, 
with the positive x-axis pointing forward, positive y 
pointing right, and positive z pointing down. There is a 
singularity in the Euler angle representation at 0 = ±90°, 
but this was not found to produce any noticeable distur- 
bances in practice. 

All the remaining references except [16] also include 
angular rates in the state vector and gyroscopic angular 
rate measurements in the measurement vector. This is 
very natural, as it allows the Euler angle integration 
kinematics [20], 

0(0 = W a (0(r)) a>(f) 
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to be incorporated into the system dynamics model, and 


allows the gyro measurements to be utilized in the 
obvious way - as measurements. However, while it is 
obvious from (6) how the derivatives of the orientation 
state elements will be computed from the state, how shall 
the derivatives of the angular velocity components depend 
on state? Some authors [18, 19] simply assume zero 
dependence, i.e. constant angular rates. Some process 
noise is added to the angular accelerations to allow for 
non-constant angular rates, but in reality the angular 
accelerations would not be very much like white noises, so 
this model cannot be very optimal. Other authors [15, 17] 
augment the state vector with cb, which changes the 
model to an assumption of constant angular acceleration. 
The difference between the true to and the assumed 
to = 0 is closer to white noise. Further derivatives, as in 
[15], make the model even more accurate, but lead to an 
unreasonably large state vector. 

For most accurate estimation, the equations of motion 
of the body being tracked should be included in the 
system dynamics model (1). For example, in [14] the 
angular accelerations of the aircraft depend precisely, 
through well-known aircraft equations of motion, on 
quantities in the state vector and aileron positions, which 
are known inputs. Unfortunately, head accelerations are 
driven by muscle forces - an unknown input - so head 
dynamics are not modeled in the current system. 

In inertial navigation applications, such as [13-15], 
gyro bias terms are usually included in the state vector. 
This is very important where the only aiding comes from 
sparse or indirect sources such as occasional position 
fixes. In this case, it is desired to milk as much accuracy 
as possible out of the gyro integration algorithm, and 
time-varying gyro biases are the largest source of error. 
Our state vector is therefore augmented with the three 
gyro bias terms 5co*, ScDy, and 8ov 

42 Complimentary Kalman filter 

While most of the references above used a Kalman 
filter to direcdy estimate the state variables of orientation 
and it’s derivatives, it is common in inertial navigation 
systems to instead use a complimentary Kalman filter 
which operates only on the errors in those primary state 
variables [12]. 

The direct Kalman filter block diagram in Figure 2 has 
co measured by the gyros and 0 measured by the aiding 
sensors all as measurement inputs. The Euler angle 
integration of (6) is then accomplished as part of the 
prediction step inside the Kalman filter block. The 
complimentary Kalman filter is shown in Figure 3. Here, 
the integration of the Euler angles is performed outside of 
the Kalman filter, in the block labeled “attitude computa- 
tion”. One advantage of this structure is that it guaran- 


188 


Sto 



Figure 2: Direct Kalman filter for orientation 


Section 4.5 describes a complimentary 
EK F to operate on the errors of the attitude 
computation with the computational 
complexity of the EKF reduced by 
applying Friedland’s separate bias 
formulation. 

The continuous-time (CT) nonlinear 
differential equation which the attitude 
computer must integrate was given in (6). 
To derive the DT attitude computation 
from it, it is useful to approximate the 
evolution of 0(t) over a short time interval 
by its Taylor series expansion 

0 (/ + Ar) = e(f) + e(/)A/ + 0 (O-^-+... ( 9 ) 



The number of terms which must 
be retained depends on the size of 
At. For a first order integration 
algorithm (retaining only the first 
two terms), the error per step will 
be mostly due to the third term, 
which is of order a) 2 Af 2 /2. 
Therefore, 

1 

error rate ~ —co At. 
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Figure 3: Complimentary Kalman filter for orientation 

tees that the rapid dynamic response of the inertial system 
will not be compromised by the Kalman filter. Another 
advantage is that the gyro rates are not treated as 
measurements, so it is unnecessary to include co in the 
state vector. Since the head dynamics are not being 
modeled in this implementation, co is excess baggage, and 
by removing it from x the dimension is reduced from 9 to 
6, with more than a three-fold computational savings. The 
following sections, therefore, will strive to develop a 
complimentary Kalman filter to estimate 

5x = ^js[5^ 88 &p 5a x 6(0 y 5a),f (7) 
using 

Sy ~ inclinometrr ~~ W ^ inclinometer ~ ^ 0 compass 

as the measurements, where 80 represents the error in the 
output of the attitude computer, and 5o> represents the 
gyro biases. 

4.3 DT nonlinear attitude computation 


For typical peak head velocities of 
about 6 radians/sec and a timestep 
of 0.003 sec, this yields an error rate of about .05 rad/s 
(about 3°/s) which is unacceptable. Retaining the third 
term, the error rate will be dominated by the fourth term, 
or order <y 3 A/ 3 / 6, so 

1 3 a 2 

error rate «— Q) At . 

6 

For the same <o and At the error rate would be about 
0.0003 rad/s, or about l°/min. Since the low-cost gyros 
are unlikely to have performance much better than this, a 
second order integration algorithm was selected. 

Differentiating (6) by the chain rule for partial deriva- 
tives results in 

e(') = -|r[ W B( e (')) <*(')] e(0 + 

M ( 10 ) 

-^[w s (0(O) ©(/)] cb(r) 

Defining (with time indices suppressed for brevity) 


A Kalman filter which operates on the errors of the 
INS attitude computer must mimic the noise-free error 
dynamics of the attitude computation. This section derives 
the attitude integration algorithm, Section 4.4 linearizes 
the attitude algorithm to obtain the error dynamics, and 
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and approximating the derivative of ©(t) by its first 
difference, 


O » + fr )- M (0 

At 

and substituting (11) and (12) into (10) yields 
6(0 = V, (0(f), ©(f)) W a (9(0) ©(/) 


( 12 ) 


+W a(9(0) 


©(/ + At) -©(f) 
At 


(13) 


Plugging (6) and (13) into (9) and rearranging terms 
slightly leads to 


0(r + At) = 0(r) + W, 


+V b W b G)(0— 


©(r)+©(/ + A/) 


At 2 


Ar 


(14) 


which is the second order DT integration step formula 
implemented in the attitude computer. Since At remains 
as an explicit parameter in this formula, it is unnecessary 
to have constant stepsize. This eliminates the difficulties 
of an interrupt driven program structure that would be 
necessary to have constant sampling rate data acquisition. 


4.4 DT linearized error dynamics 


Equation (14) defines a nonlinear state propagation 
function f A , for the system with state vector 0 and input ©: 
0(f + At) = t^ (0(0, ©(f), ©(/ + At), 0 (15) 


For the sake of obtaining an extended Kalman filter 
which can estimate both orientation errors and gyro 
biases, consider augmenting the state vector with © and 
rewriting the system in the form 
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where u(t) has been deviously chosen to make ©( t) evolve 
in accordance with the input history of the previous 
system. The system error dynamics can now be obtained 
by linearizing about the nominal trajectory [0(f) ©(f)] 


to get 
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and 0 and I are 3-by-3 zero and identity matrices. The 
vector partial derivatives of V B are too messy to write out 
in full, but the computation is straightforward and can be 
carried out as follows: 1) form a “row vector*’ of the three 
matrices obtained by differentiating V B with respect to the 
first, second and third elements of the vector in the 
denominator of the partial derivative; 2) multiply each of 
these three matrices by the r.h.s. vector W B ©. This results 
in a “row vector of column vectors”, i.e. a 3-by-3 matrix. 

Equation (17) gives the state transition matrix for the 
linearized error dynamics of the augmented system. The 
angular velocity errors 5© are principally due to gyro 
biases, and will be interpreted simply as gyro biases from 
here on. The A and B submatrices can be interpreted as 
describing the influence of the orientation error and gyro 
biases at time t on the orientation error at time f+Af. The 
effect of the matrix is fairly obvious; it basically mimics 
the attitude computation of (14) except that the input 
angular velocity is due to gyro biases and the output is 
therefore an orientation error . The growth of orientation 
error in the absence of angular rate errors is governed by 
the A matrix. To first order A = I+VjAf. The identity 
term maintains the previously accrued error, and Vb( 9,©) 
amplifies existing orientation errors in response to 
motion. 

4.5 Separate-bias Kalman filter formulation 

The linear error propagation model of (17) provides 
the basis for a complimentary Kalman filter to estimate 
these errors. The model has been manipulated into a form 
in which the gyro biases are assumed constant, thus 
permitting the direct application of Friedland’s separate- 
bias Kalman filtering results [21]. If the constant-bias 
model turns out to fit the gyro performance poorly, the 
restriction can later be ameliorated by use of an age- 
weighting factor. If an exponential gyro warm-up model 
as in [15] seems more appropriate, this can be accommo- 
dated within Friedland’s formulation by replacing the 
identity submatrix in the state transition matrix of (17). 

Switching to Friedland’s notation, define an error state 
vector x k s S0(/ A ) and a bias state vector b k s 5©(r* ) 
where t* is the time at the k* iteration of the algorithm. 
An augmented state vector z k = [x A b k ] satisfies 
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The additive white noise w k , with variance Q k , only 
effects x, since b is assumed constant. The measurement 
equation is 

y* = L 4 z A +v 4 , (20) 

where v k is white noise with variance Q k . In Friedland’s 
paper, L k = [H t Cj. but in this application the 
measurements from the inclinometers and compass only 
measure x and not b, so C = 0 will be used throughout, 
resulting in a great simplification from Friedland’s 
derivation. 

Applying Kalman filtering to this model, the optimal 
estimate of z is 

z t+1 =F t z t +K(A: + lXy* +1 -LF t 2 4 ) (21) 

K(k) = P(*)L r [LP(Jfc)L r + R t . (22) 

The Ricatti equations for the recursive computation of the 
estimation error covariance matrix P(k) needed in the 
Kalman gain expression can be rolled together into the 
single predictor-to-predictor covariance update equation: 

P(* + l) = F t [l-K(*)L]P(*)F t r +[J]q* + ,[I «].(23) 

Partitioning P(k) into 3-by-3 submatrices as 

[ r.«0 P.Ml 

the expression for the Kalman gain, (22), may be 
rewritten in partitioned form as 


*,(*)] = v x m T [^m T + rT 

K »( k )\ " P/(A:)H r [HP I (t)H r + R,] ' 


These separate gains are used in two essentially separate 
Kalman filters, one for estimating x and one for b. To 
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compute the K, and gains in (25), covariance sub- 
matrices P* and P xb are needed. These are updated by the 
partitioned version of (23): 

[£■ ?]-[* til: :H3" 4 

[p, p*1[a 4 B.1 i-q. 0- 

P / P, II 0 I I I o 0 
L * * JL , (26) 

rA 4 -A t K,H-B 4 K„H B, 

“L -k* h l \ 

rp^z+p^B/ pjro, o' 

[p^AZ + P.B/ P 6 J 1° 0. 

Thus, a plethora of 6-by-6 matrix multiplications and 
one 6-by-6 inversion are replaced by a somewhat greater 
number of 3-by-3 multiplications and one 3-by-3 inver- 
sion. 

5. Implementation 

Figure 4 illustrates the configuration of the hardware 
built to demonstrate the inertial head-attitude tracking 
concept The sensors are all embedded in a specially 
machined 2” X 2" X 1.25" plastic block connected by a 
thin 10’ cable to an analog signal conditioning circuit and 
data acquisition card in a PC. 

Software was written in ‘XT’* to run on the PC and 
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Figure 4: Orientation tracker hardware configuration. 


Figure 5: Inertial orientation tracker main software loop. 

The initialization block, executed once 
at program start-up, sets the initial state 
estimates and covariances as follows: 

Xo: The inclinometer is read and used 

6dx33 to set \y and 0. The compass, if used, 

determines ij); otherwise <J)=0. 

iel f bo: The biases of all 3 gyros are meas- 

JD ured during system calibration and stored 

ercard in a file. On initialization, the file is read 

and 5co is initialized with the stored gyro 

biases. 

P x (0): The errors in the initial deter- 
mination of the Euler angles may be 
substantial, but they are assumed to be 
uncorrelated with one another: P x (0) = I. 
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P b (0): The gyro biases at start-up could differ substan- 
tially from the prerecorded calibration values, but the 
uncertainties are uncorrelated: P b (0) = 0. II. 

Pxb(0): The initial uncertainties in orientation and gyro 
bias are completely uncorrelated: P xb (0) = 0. 

The data acquisition block scans all the A/D channels 
in rapid succession. The new gyro readings are stored as 
co(t+At) and the previous ones are moved back to co(t). 
The new inclinometer and compass readings are stored in 
y(t+At). In the next block, a timestamp is obtained from 
the 8253 timer/counter chip on the PC motherboard. This 
counter is driven by a 1.19 MHz oscillator with a 65,536 
divisor to generate 18.2 Hz timer ticks for BIOS and DOS 
time-keeping. By reprogramming the divisor it was found 
possible to obtain sub-microsecond timing resolution as 
required for inertial integration. At is calculated as the 
difference between the current timestamp and the 
previous one. 

Next, co(t), co(t+At) and At are fed into the Kalman 
filter update block. W B and V B are computed and then 
used in (14) to compute the predicted 0(t+At). This 
corresponds to the attitude computation block in. Since 
the Euler angle estimates, 0 must be maintained anyway, 
it is convenient to subsume 60 into them, and keep track 
of total estimates only. This does not change the filter 
framework developed in the previous section in any 
important way; it just means that 5 0(r) is always zero at 
the beginning of each iteration of the Kalman filter. At 
the end of the Kalman filter update cycle, 50(f + Af) is 
used to reset 0(r + A/) and then flushed back to zero 
before the next cycle. Since the attitude error estimates 
are propagated along with the attitude estimates through 
the nonlinear propagation equation, the top three 
elements of F t z* in (21) are replaced with zeros. Since co 
is not included in the state, the running estimates of 8a> 
must still be kept track of in the Kalman filter. They are 
propagated through the prediction step unchanged, as 
governed by the bottom three rows of F k . The system, 
then , can be thought of as a mixture of a purely compli- 
mentary Kalman filter as described in the previous section 
and an extended Kalman filter which keeps track of total 
estimates of state. 

The next stage in the computational loop is to incorpo- 
rate the measurements and update the error estimates as 
follows 

se t+l =8e t+1 +K,(*+i)v t+l 
= 6a> t+l +K,(* + l)v„, 

where v k+ i is the innovations obtained by subtracting the 
predicted orientation estimates from the new orientation 
measurements. In order to calculate K x (k+1) and Kt,(k+1) 
with equation (25) it is necessary to first propagate the 


covariance submatrices using (26). Since the inclinometer 
and compass signals are pre-processed to give direct 
measurements of the Euler angles, H=I, and (26) is 
simplified to the following steps: 

T, = A-AK, 

Ti = T|Prf, 

T, = BT, r 


= Tj + BP, 


(28) 


P,/ = Tj + BP, + 

P/ =P jt t B r +T 3 +T 1 P,A r 

where Tj are simply temporary storage matrices used to 
reduce the amount of redundant matrix multiplication. A 
small subroutine library was written, following the pointer 
conventions and numerical methods described in [22], to 
perform the necessary matrix multiplication, transposi- 
tion, addition and inversion operations to carry out these 
steps. 


5.1 The Q k and R k Matrices 

Ideally, Qk is supposed to reflect the magnitude of a 
white noise sequence. If all error sources in the inertial 
attitude system are taken care of (i.e. modeled in the state 
propagation matrix), then w k in (19) should be entirely 
due to the noise floors of the angular rate sensors. In this 
case, it should be possible to calculate the optimal value of 
Q k by measuring the noise covariance, Q, of the station- 
ary gyros in advance, then at each time step compute 
Q t = G,QG t r , using G, = W 4 (0(f 4 )). 

However, there are many nonwhite error sources be- 
sides bias, such as nonlinearity, hysteresis, misalignment, 
g-sensitivity, and scale factor temperature coefficient, 
none of which are modeled in the current implementation. 
The best procedure for designing a reduced-order Kalman 
filter under these circumstances is to use a Schmidt- 
Kalman filter, which eliminates the unmodeled states 
from the state vector, but continues to propagate their 
covariances in partitioned Ricatti equations and Q and 
R matrices. A simpler approach, which sometimes works 
almost as well [12, p. 397], is to just ignore the unmod- 
eled states, but “bump up” the Q and R matrices to 
account for the noises in the states being discarded. This 
approach is taken here, except the “bumping up” is done 
in a very inexact way. 

Without having a model of the gyro dynamics, the 
following error sources in the process equation (19) are 
assumed: 

gyro noise: From an oscilloscope, for stationary gyros, 
a=0.01 rad/s. Covariance per step (0.01 At) 2 . 

integration rule error: From the analysis in Section 
4.3, a=G) 3 At 2 rad/s. Covariance per step co 6 At 6 . 
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scale factor error: This is a composite of nonlinearity 
and temperature dependent scale factor variations. 
Assuming scale factor accuracy of 1% of full scale, 
a=0.01 co rad/s. Covariance per step (O.OlcoAt) 2 . 

Assuming At=0.01sec, and that these error sources are 
uncorrelated, the error covariances add up to approxi- 
mately 10’*(1 +cd 2 +10' 4 <d 6 ). At each iteration of the 
Kalman filter software, the following algorithm is used to 
compute Q k : 

1. find ow = maxfcojoCOy.co*) 

2. set a w 2 = 10^(1 +ow 2 +1 0'C) 


0 


3. set Q, = 


0 

0 


0 

0 


4. set R, = 


0 

0 


0 

0 . 

o. 


According to this algorithm, the measurement error 
covariances for the inclinometer roll and pitch range from 
1, during periods of likely slosh, down to 10 -4 , during 
periods of likely stillness. The covariance of the compass 
yaw error only comes down to 0.0 1 , corresponding to o = 
6°, because even with good inclinometer information, 
magnetic distortions in the room make the compass this 
inaccurate. 


6. Results 


4. set Q t = W B Q t W/ 

This algorithm is very crude and likely to overestimate 
Q k because it uses au to find the variance for all three 
diagonal elements of Q k . 

Rk is modeled in an equally sloppy manner. The meas- 
urement noise is extremely nonwhite. The major source of 
measurement noise for the fluid inclinometers is “slosh” 
caused by transverse linear accelerations. Linear motion 
is not included in the state vector, and therefore, this error 
cannot be modeled in the measurement matrix. Further- 
more, the magnitude of the low-frequency “slosh” errors 
are sometimes extremely large: up to 1 radian. Slosh- 
induced inclination errors cause similarly large heading 
errors in the compass system. On the other hand, when 
the head is still, there is no slosh and the attitude angles 
measured by the inclinometer are very accurate. The 
algorithm for Rk is therefore designed in a heuristic way 
to force the Kalman filter to take good advantage of the 
measurements when they are likely to be meaningful, and 
to ignore them when they are likely to be erroneous. The 
basic principle is that <J r should approach 1 when slosh is 
likely, and approach the static accuracy of the inclinome- 
ter/compass measurements, about 0.01 radians, when 
slosh is very unlikely. In the absence of a model for 
human head motion, it is assumed that a person cannot 
sustain a constant linear acceleration of the head very 
long with no rotation. Therefore, the longer the period of 
time that the head has had 1) zero angular velocity, and 
2) unchanging inclinometer outputs, the higher the 
probability that the head is still. Based on this intuition, 
the algorithm used to set Rk is: 

1 . compute “stilltime”, x, since last non-zero gyro 
reading OR last change in inclinometer reading. 

2. set 0 V = l/(l + 400r) 

3. if o r < 0.01 , set 0 V = 0.01 


Using the Q k and Rk matrices described above, it was 
found that the Kalman filter diverged within a few 
seconds when the sensor was still. An age weighting 
multiplier did not help. After much experimentation, it 
was found that the only way to prevent divergence is to 
never let the diagonal elements of Rk be less than 1 . The 
algorithm for Rk was adjusted so that a® ranges from 10, 
when x=0, to 1, when x>0.2. The base level of Q k was also 
boosted from 10 * to 10* 4 so that the filter would still make 
use of the measurements with the larger measurement 
noise covariance. With these modifications, the filter 
remains stable indefinitely and succeeds in eliminating 
long term drift without compromising the rapid dynamic 
response of the inertial tracking technique. The filter can 
run at approximately 200 iterations/second. This is a five- 
fold slowdown as compared to the raw attitude computa- 
tion with the Kalman filtering steps commented out. 
However, it is still reasonably fast and the delay can be 
compensated for by prediction if necessary. 



Figure 6: Test run without complimentary Kalman filter. 

To demonstrate the behavior of the filter, two datasets 


were collected. In the first dataset, the complimentary 


193 



Kalman filter block is disabled by setting K x and K*, equal 
to zero. During the test period of approximately 35 
seconds, the sensor block was repeatedly turned through 
+90° about the roll axis and left to rest on its right side, 
then returned to rest in its horizontal orientation on the 
table. The roll Euler angle is plotted against time in 
Figure 6, which demonstrates the problem with unaided 
inertial integration: the accumulated drift error by the end 
of the run is about 15°. The second dataset is created by a 
similar motion sequence, but the Kalman filter is in 
effect As Figure 7 shows, the filter incorporates the drift- 
free but noisy measurements from the inclinometers, and 
effectively compensates the drift of the inertial system. 
Due to the time-varying R* strategy which shuts out the 
measurements during motion, a certain amount of error 
accumulates each time the sensor is rolled over and back, 
and the Kalman filter corrects it once the sensor returns to 
a stationary pose. The graph clearly shows the time- 
course of this corrective action. 



Figure 7: Test run with complimentary Kalman filter. 
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